using System;
using System.Collections.Generic;
using System.Text;
using DotNetMatrix;
using Blimpbots.Recognition;


namespace Blimpbots.Recognition.Algorithms
{
    public class ProjectiveKalman : KalmanFilter
    {
        private double _last_t;
        double _stDev_x, _stDev_y, _stDev_z;

        public ProjectiveKalman(double stDev_x, double stDev_y, 
                                double stDev_z, double init_stDev, Point3D x, double t)
        {
            _F = GeneralMatrix.Identity(8, 8);

            _stDev_x = stDev_x;
            _stDev_y = stDev_y;
            _stDev_z = stDev_z;

            _last_t = t;

            // Note that we have no certainty about the position
            _P = GeneralMatrix.Identity(8, 8).MultiplyEquals(init_stDev*init_stDev);
            _P.Array[3][3] = 0; // no uncertainty about scale

            _x = new GeneralMatrix(new double[] {x.X, x.Y, x.Z, 1.0, 0, 0, 0, 0}, 8);
           
        }

        public Point3D Update(GeneralMatrix C, Point3D z, double meas_stDev_x, double meas_stDev_y,
                              double meas_stDev_z, double t)
        {
            #region Convert Z to Matrix
            GeneralMatrix zMat = new GeneralMatrix(new double[] {z.X, z.Y, z.Z}, 3);
            #endregion

            double dT = t - _last_t;
            _last_t = t;

            #region Prepare Q
            _Q = CalcQ(dT);
            #endregion

            #region Prepare F

            // Position estimate is the same as before 
            //   plus velocity times dT
            _F.Array[0][4] = dT;
            _F.Array[1][5] = dT;
            _F.Array[2][6] = dT;

            #endregion

            #region Predict and Prepare H

            GeneralMatrix B = new GeneralMatrix(3, 8);
            B.SetMatrix(0, 2, 0, 3, C);

            GeneralMatrix _xHat = Predict();
            
            // TODO: not sure about this!!
            double alphaScalar = 1.0 / 
                ( C.Array[2][0] * _xHat.Array[0][0] 
                + C.Array[2][1] * _xHat.Array[1][0] 
                + C.Array[2][2] * _xHat.Array[2][0]
                + C.Array[2][3] * _xHat.Array[3][0]);

            GeneralMatrix Alpha = GeneralMatrix.Identity(3, 3).MultiplyEquals(alphaScalar);

            _H = Alpha * B;

            #endregion

            #region Prepare R
            _R = GeneralMatrix.Identity(3, 3);
            _R.Array[0][0] = meas_stDev_x * meas_stDev_x;
            _R.Array[1][1] = meas_stDev_y * meas_stDev_y;
            _R.Array[2][2] = meas_stDev_z * meas_stDev_z;
            #endregion

            #region Update
            GeneralMatrix x = Update(zMat);
            #endregion

            // TODO: normalize?
            #region Convert Z to Point3D
            Point3D result = new Point3D(x.Array[0][0], x.Array[1][0], x.Array[2][0]); 
            #endregion

            return result;
        }

        private GeneralMatrix CalcQ(double dT)
        {
            GeneralMatrix G = new GeneralMatrix(new double[8][] {
                new double[1] { (dT*dT)/2 }, // x
                new double[1] { (dT*dT)/2 }, // y
                new double[1] { (dT*dT)/2 }, // z
                new double[1] { 0 },         // scale
                new double[1] { dT },        // x dot
                new double[1] { dT },        // y dot
                new double[1] { dT },        // z dot
                new double[1] { 0 },         // 
                });

            GeneralMatrix Sigma = new GeneralMatrix(new double[] {
                _stDev_x,
                _stDev_y,
                _stDev_z,
                0,
                _stDev_x,
                _stDev_y,
                _stDev_z,
                }, 1);

            GeneralMatrix Q = new GeneralMatrix(8, 8);
            Q.Array[0][0] = _stDev_x * (dT * dT) / 2;
            Q.Array[1][1] = _stDev_y * (dT * dT) / 2;
            Q.Array[2][2] = _stDev_z * (dT * dT) / 2;

            Q.Array[4][4] = _stDev_x * dT;
            Q.Array[5][5] = _stDev_y * dT;
            Q.Array[6][6] = _stDev_z * dT;

            return Q;
        }




    }
}
